#include "can_cfg.h"
#include "can.h"
#include "stdio.h"
#include "semphr.h"
#include "pc_interaction.h"

//标识符
//脉塔电机 --- 0x140 + 1 
//关节电机 --- 1 、 2

//位数
//脉塔电机 --- 8位
//关节电机 --- 5

extern CAN_HandleTypeDef hcan1;
CAN_TxHeaderTypeDef TXHeader;
CAN_RxHeaderTypeDef RXHeader;
extern osSemaphoreId_t joint_ang_semHandle;
extern osEventFlagsId_t deal_ang_eventHandle;
uint8_t buf[255] = {0};
uint8_t instruct_code = 0;
int64_t callback_ang = 0;
float Joint1_real_callback_ang = 0;
float Joint2_real_callback_ang = 0;
uint8_t test_can_init_result = 5;
uint8_t CAN_ID = 0;

double ratio = 0;

/*整数到整数映射函数  将输入量 映射到不同单位的输出*/
int32_t map_intoin(int32_t x, int32_t in_min, int32_t in_max, int32_t out_min, int32_t out_max)                
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

//整数到浮点数映射
float map_intoflo(int32_t x, int32_t in_min, int32_t in_max, float out_min, float out_max)                
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

//浮点数到整数映射
int32_t map_float_to_int(double x, double in_min, double in_max, int32_t out_min, int32_t out_max)
{
    if (in_max == in_min) return out_min;  // 防止除零
    ratio = (x - in_min) / (double)(in_max - in_min);  // 先计算比例
    return (int32_t)(ratio * (out_max - out_min) + 0.5f) + out_min;  // 四舍五入
}


void can_filter(void)
{
    CAN_FilterTypeDef  sFilterConfig;

    sFilterConfig.FilterBank = 0;//过滤器0 这里可设0-13
    sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;//采用掩码模式
    sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;//采用32位掩码模式
    sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO0;//采用FIFO0
    
    sFilterConfig.FilterIdHigh = 0x0000; //设置过滤器ID高16位
    sFilterConfig.FilterIdLow = 0x0000;//设置过滤器ID低16位
    sFilterConfig.FilterMaskIdHigh = 0x0000;//设置过滤器掩码高16位
    sFilterConfig.FilterMaskIdLow = 0x0000;//设置过滤器掩码低16位
    
    sFilterConfig.FilterActivation = ENABLE;//打开过滤器
    sFilterConfig.SlaveStartFilterBank = 14;

    HAL_CAN_ConfigFilter(&hcan1,&sFilterConfig);//初始化过滤器
    test_can_init_result = HAL_CAN_Start(&hcan1);//打开can   ---- 不接设备调试会打开失败
    HAL_CAN_ActivateNotification(&hcan1,CAN_IT_RX_FIFO0_MSG_PENDING);//开启接受邮箱0挂起中断
    
}

void canCallback_datadeal()
{
  // instruct_code = buf[0];
  uint8_t effect_pos = 0;
  callback_ang = 0x00;
  // switch(instruct_code)
  // {
    // case(0x08):
    // {
      for(int8_t i = 7; i >= 1; i --) //默认最大64位
      {
        //从高位到低位寻找第一个有效字节
        if(buf[i] != 0x00 && buf[i] != 0xFF) continue;

        if(i == 8) return; //数据溢出

        effect_pos = i;
        break;
      }

      //判断正数还是负数 选择补位数据
      if(buf[effect_pos + 1] == 0x00) //正
      {
          for(int8_t j = effect_pos; j >= 1; j --)
          {
            callback_ang |= buf[j] << ((j - 1) * 8);
          }
      }
      else if(buf[effect_pos + 1] == 0xFF) //负
      {
          for(int8_t k = effect_pos + 1; k < 9; k ++)
          {
            callback_ang |= 0xFF << (k * 8);
          } 
          for(int8_t j = effect_pos; j >= 1; j --)
          {
            callback_ang |= buf[j] << ((j - 1) * 8);
          }
      }

      //实际角度折算
      // if(xSemaphoreTakeFromISR(joint_ang_semHandle, NULL) == pdPASS)
      // {
      if(CAN_ID == 0x01)
      {
          Joint1_real_callback_ang = map_intoflo(callback_ang, Joint_1_Angle_Min_Limit, Joint_1_Angle_Max_Limit, 1, 360);
      }
      else if(CAN_ID == 0x02)
      {
          Joint2_real_callback_ang = map_intoflo(callback_ang, Joint_2_Angle_Min_Limit, Joint_2_Angle_Max_Limit, 0, 180);
      } 

      // xSemaphoreGive(joint_ang_semHandle);  
      // }
           
    // };break;
  // }
// }
}

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)//接受邮箱0挂起中断回调函数
{
	uint8_t	RxData[8];
	if(hcan->Instance==CAN1)
	{
    HAL_CAN_GetRxMessage(hcan,CAN_FILTER_FIFO0,&RXHeader,RxData);//获取数据
    memcpy(buf, RxData, RXHeader.DLC); 
    if(buf[0] == 0x08)
    {
        CAN_ID = RXHeader.StdId;
        osEventFlagsSet(deal_ang_eventHandle, deal_ang_time_event);
    }
        
  }
}

/***************************************************************************************************/
/***************************************************************************************************/
/***************************************************************************************************/
/***************************************脉塔电机控制CAN报文******************************************/
/***************************************************************************************************/
/***************************************************************************************************/
/***************************************************************************************************/
/*转矩闭环控制命令
转矩控制低字节
转矩控制高字节
*/
void RMD_TorgueControl(uint8_t Motor_ID, int32_t iqControl)
{
  uint8_t TxData[RMD_CAN_DLC] = {0};
  uint32_t TxMailbox = CAN_TX_MAILBOX0;
  TXHeader.RTR = CAN_RTR_DATA;
  TXHeader.IDE = CAN_ID_STD;            
  TXHeader.StdId = std_id + Motor_ID;
  TXHeader.TransmitGlobalTime = DISABLE;
  TXHeader.DLC = RMD_CAN_DLC;
    
  TxData[0] = RMD_Torque_Closed_Loop_Control_Command;
  TxData[1] = 0;
  TxData[2] = 0;
  TxData[3] = 0;
  TxData[4] = *(uint8_t *)(&iqControl); 
  TxData[5] = *((uint8_t *)(&iqControl)+1);
  TxData[6] = 0;
  TxData[7] = 0;
        
  HAL_CAN_AddTxMessage(&hcan1, &TXHeader, TxData, &TxMailbox);
}

/**
  * @brief  speeed control A2 command
  * @param  uint8_t Motor_ID 
  * @param  int32_t speedControl
  */
void RMD_speedControl(uint8_t Motor_ID, int32_t speedControl)  
{
  uint8_t TxData[RMD_CAN_DLC] = {0};
  uint32_t TxMailbox =CAN_TX_MAILBOX0;
  TXHeader.RTR = CAN_RTR_DATA;
  TXHeader.IDE = CAN_ID_STD;            
  TXHeader.StdId = std_id + Motor_ID;
  TXHeader.TransmitGlobalTime = DISABLE;
  TXHeader.DLC = RMD_CAN_DLC;
    
  TxData[0] = RMD_Speed_Closed_Loop_Control_Command;
  TxData[1] = 0;
  TxData[2] = 0;
  TxData[3] = 0;
  TxData[4] = *(uint8_t *)(&speedControl); 
  TxData[5] = *((uint8_t *)(&speedControl)+1);
  TxData[6] = *((uint8_t *)(&speedControl)+2);
  TxData[7] = *((uint8_t *)(&speedControl)+3);
        
  HAL_CAN_AddTxMessage(&hcan1, &TXHeader, TxData, &TxMailbox);

}

/**
  * @brief  Multi_angleControl_1 A3 command
  * @param  uint8_t Motor_ID 
  * @param  int32_t angleControl
  */
void RMD_Multi_angleControl_1(uint8_t Motor_ID, int32_t angleControl)
{
  uint8_t TxData[RMD_CAN_DLC] = {0};
  uint32_t TxMailbox =CAN_TX_MAILBOX0;
  TXHeader.RTR = CAN_RTR_DATA;
  TXHeader.IDE = CAN_ID_STD;            
  TXHeader.StdId = std_id + Motor_ID;
  TXHeader.TransmitGlobalTime = DISABLE;
  TXHeader.DLC = RMD_CAN_DLC;
    
  TxData[0] = RMD_Pos_Closed_Loop_Control_Command_1;
  TxData[1] = 0;
  TxData[2] = 0;
  TxData[3] = 0;
  TxData[4] = *(uint8_t *)(&angleControl); 
  TxData[5] = *((uint8_t *)(&angleControl)+1);
  TxData[6] = *((uint8_t *)(&angleControl)+2);
  TxData[7] = *((uint8_t *)(&angleControl)+3);
  HAL_CAN_AddTxMessage(&hcan1, &TXHeader, TxData, &TxMailbox);

}
/**
  * @brief  Multi_angleControl_1 A4 command
  * @param  uint8_t Motor_ID 
  * @param  uint16_t maxSpeed
  * @param  int32_t angleControl
  */
void RMD_Multi_angleControl_2(uint8_t Motor_ID, uint16_t maxSpeed, int32_t angleControl)
{
  uint32_t TxMailbox =CAN_TX_MAILBOX0;
  uint8_t TxData[RMD_CAN_DLC] = {0};
  
  TXHeader.RTR = CAN_RTR_DATA;
  TXHeader.IDE = CAN_ID_STD;            
  TXHeader.StdId = std_id + Motor_ID;
  TXHeader.TransmitGlobalTime = DISABLE;
  TXHeader.DLC = RMD_CAN_DLC;
    
  TxData[0] = RMD_Pos_Closed_Loop_Control_Command_2;
  TxData[1] = 0;
  TxData[2] = *(uint8_t *)(&maxSpeed);
  TxData[3] = *(uint8_t *)((&maxSpeed)+1);
  TxData[4] = *(uint8_t *)(&angleControl); 
  TxData[5] = *((uint8_t *)(&angleControl)+1);
  TxData[6] = *((uint8_t *)(&angleControl)+2);
  TxData[7] = *((uint8_t *)(&angleControl)+3);

  HAL_CAN_AddTxMessage(&hcan1, &TXHeader, TxData, &TxMailbox);

}
/**
  * @brief  Single_loop_angleControl_1 A5 command
  * @param  uint8_t Motor_ID 
  * @param  uint8_t spinDirection  控制电机的方向 0x00顺时针 0x01逆时针
  * @param  int16_t angleControl
  */
void RMD_Single_loop_angleControl_1(uint8_t Motor_ID, uint8_t spinDirection, uint16_t angleControl)
{
  uint32_t TxMailbox =CAN_TX_MAILBOX0;
  uint8_t TxData[RMD_CAN_DLC] = {0};
  
  TXHeader.RTR = CAN_RTR_DATA;
  TXHeader.IDE = CAN_ID_STD;            
  TXHeader.StdId = std_id + Motor_ID;
  TXHeader.TransmitGlobalTime = DISABLE;
  TXHeader.DLC = RMD_CAN_DLC;
    
  TxData[0] = RMD_Pos_Closed_Loop_Control_Command_3;
  TxData[1] = spinDirection;
  TxData[2] = 0;
  TxData[3] = 0;
  TxData[4] = *(uint8_t *)(&angleControl); 
  TxData[5] = *((uint8_t *)(&angleControl)+1);
  TxData[6] = 0;
  TxData[7] = 0;
        
  HAL_CAN_AddTxMessage(&hcan1, &TXHeader, TxData, &TxMailbox);
}
/**
  * @brief  Single_loop_angleControl_2 A6 command
  * @param  uint8_t Motor_ID 
  * @param  uint8_t spinDirection  转动方向
  * @param  uint16_t maxSpeed  转动速度
  * @param  int16_t angleControl  转动角度
  */
void RMD_Single_loop_angleControl_2(uint8_t Motor_ID, uint8_t spinDirection, uint16_t maxSpeed, uint16_t angleControl)
{
  uint32_t TxMailbox =CAN_TX_MAILBOX0;
  uint8_t TxData[RMD_CAN_DLC] = {0};
  TXHeader.RTR = CAN_RTR_DATA;
  TXHeader.IDE = CAN_ID_STD;            
  TXHeader.StdId = std_id + Motor_ID;
  TXHeader.TransmitGlobalTime = DISABLE;
  TXHeader.DLC = RMD_CAN_DLC;
    
  TxData[0] = RMD_Pos_Closed_Loop_Control_Command_4;
  TxData[1] = spinDirection;
  TxData[2] = *(uint8_t *)(&maxSpeed);
  TxData[3] = *((uint8_t *)(&maxSpeed)+1);
  TxData[4] = *(uint8_t *)(&angleControl); 
  TxData[5] = *((uint8_t *)(&angleControl)+1);
  TxData[6] = 0;
  TxData[7] = 0;
  
  HAL_CAN_AddTxMessage(&hcan1, &TXHeader, TxData, &TxMailbox);
}

/**
  * @brief  RMD_Mul_abs_loop_angleControl A4 command
  * @param  uint8_t Motor_ID 
  * @param  uint8_t spinDirection  转动方向
  * @param  uint16_t maxSpeed  转动速度
  * @param  int16_t angleControl  转动角度
  */
void RMD_Mul_abs_loop_angleControl(uint8_t Motor_ID, uint16_t maxSpeed, uint32_t angleControl)
{
  uint32_t TxMailbox = CAN_TX_MAILBOX0;
  uint32_t angleControl_real;
  if(angleControl >= 360)  angleControl_real = 180;
  else if(angleControl <= 0) angleControl_real = 0;
  else angleControl_real = angleControl;
  angleControl_real = angleControl_real * 100;
  uint8_t TxData[RMD_CAN_DLC] = {0};
  TXHeader.RTR = CAN_RTR_DATA;
  TXHeader.IDE = CAN_ID_STD;            
  TXHeader.StdId = std_id + Motor_ID;
  TXHeader.TransmitGlobalTime = DISABLE;
  TXHeader.DLC = RMD_CAN_DLC;
    
  TxData[0] = RMD_Pos_Closed_Loop_Control_Command_2;
  TxData[1] = 0x00;
  TxData[2] = *(uint8_t *)(&maxSpeed);
  TxData[3] = *((uint8_t *)(&maxSpeed)+1);
  TxData[4] = *(uint8_t *)(&angleControl_real); 
  TxData[5] = *((uint8_t *)(&angleControl_real)+1);
  TxData[6] = *((uint8_t *)(&angleControl_real)+2);
  TxData[7] = *((uint8_t *)(&angleControl_real)+3);
  
  HAL_CAN_AddTxMessage(&hcan1, &TXHeader, TxData, &TxMailbox);
}

/**
  * @brief  Motor_Off command
  * @param  uint8_t Motor_ID 
  */
void RMD_Motor_Off(uint8_t Motor_ID)
{
  uint32_t TxMailbox =CAN_TX_MAILBOX0;
  uint8_t TxData[RMD_CAN_DLC] = {0};
  memset(TxData, Sizeof(TxData), 0);
  
  TXHeader.RTR = CAN_RTR_DATA;
  TXHeader.IDE = CAN_ID_STD;            
  TXHeader.StdId= std_id + Motor_ID;
  TXHeader.TransmitGlobalTime = DISABLE;
  TXHeader.DLC = RMD_CAN_DLC;
    
  TxData[0] = 0x81;
        
  HAL_CAN_AddTxMessage(&hcan1, &TXHeader, TxData, &TxMailbox);

}

void RMD_Motor_Normal_Ctr(uint8_t Motor_ID, uint8_t cmd)
{
  uint32_t TxMailbox =CAN_TX_MAILBOX0;
  uint8_t TxData[RMD_CAN_DLC] = {0};
  memset(TxData, Sizeof(TxData), 0);
  TXHeader.RTR = CAN_RTR_DATA;
  TXHeader.IDE = CAN_ID_STD;            
  TXHeader.StdId= std_id + Motor_ID;
  TXHeader.TransmitGlobalTime = DISABLE;
  TXHeader.DLC = RMD_CAN_DLC;

  TxData[0] = cmd;
  HAL_CAN_AddTxMessage(&hcan1, &TXHeader, TxData, &TxMailbox);
}


/***************************************************************************************************/
/***************************************************************************************************/
/***************************************************************************************************/
/***************************************关节电机控制CAN报文******************************************/
/***************************************************************************************************/
/***************************************************************************************************/
/***************************************************************************************************/
//关节电机id 1 2 ---->2有角度限制 ---> -100000 ~ -3000000
void Joint_CAN_SendPosition(uint8_t Motor_ID, int32_t angleControl)
{
  uint8_t TxData[Joint_CAN_DLC_5] = {0};
  
  uint32_t TxMailbox = CAN_TX_MAILBOX0;

  TXHeader.RTR = CAN_RTR_DATA;
  TXHeader.IDE = CAN_ID_STD;            
  TXHeader.StdId = Motor_ID;
  TXHeader.TransmitGlobalTime = DISABLE;
  TXHeader.DLC = Joint_CAN_DLC_5;

  if(Motor_ID == 1)
  {
    if(angleControl >= 270) angleControl = 270;
    else if(angleControl <= 160) angleControl = 160; //模拟手臂前后限位
    angleControl = map_intoin(angleControl, 1, 360, Joint_1_Angle_Min_Limit, Joint_1_Angle_Max_Limit);
  }
  else if(Motor_ID == 2)
  {
    if(angleControl >= 190) angleControl = 190;
    else if(angleControl <= 90) angleControl = 90; //模拟手臂左右限位
    angleControl = map_intoin(angleControl, 0, 180, Joint_2_Angle_Min_Limit, Joint_2_Angle_Max_Limit);
  }

  TxData[0] = Joint_Set_Pos_Model_Pos_Angle_Command;
  TxData[1] = *(uint8_t *)(&angleControl); 
  TxData[2] = *((uint8_t *)(&angleControl)+1);
  TxData[3] = *((uint8_t *)(&angleControl)+2);
  TxData[4] = *((uint8_t *)(&angleControl)+3);
       
  HAL_CAN_AddTxMessage(&hcan1, &TXHeader, TxData, &TxMailbox);
  // while(HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) != 3)
  // {
  //   printf("Joint_CAN_SendPosition Senting....");
  //   HAL_Delay(1);
  // };
	
}

void Joint_CAN_Set_Pos_Limit(uint8_t Motor_ID, int32_t Limit_P_Pos, int32_t Limit_N_Pos)
{
  uint8_t TxData[Joint_CAN_DLC_5] = {0};

  uint32_t TxMailbox = CAN_TX_MAILBOX0;
  TXHeader.RTR = CAN_RTR_DATA;
  TXHeader.IDE = CAN_ID_STD;            
  TXHeader.StdId = Motor_ID;
  TXHeader.TransmitGlobalTime = DISABLE;
  TXHeader.DLC = Joint_CAN_DLC_5;

  TxData[0] = Joint_Set_Max_Positive_Pos_Command;
  TxData[1] = *(uint8_t *)(&Limit_P_Pos); 
  TxData[2] = *((uint8_t *)(&Limit_P_Pos)+1);
  TxData[3] = *((uint8_t *)(&Limit_P_Pos)+2);
  TxData[4] = *((uint8_t *)(&Limit_P_Pos)+3);

  HAL_CAN_AddTxMessage(&hcan1, &TXHeader, TxData, &TxMailbox);
  while(HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) != 3)
  {
    printf("Joint_CAN_Set_Pos_P_Limit Senting...");
    HAL_Delay(1);
  };

  memset(TxData, sizeof(TxData), 0);
  TxData[0] = Joint_Set_Max_Positive_Pos_Command;
  TxData[1] = *(uint8_t *)(&Limit_N_Pos); 
  TxData[2] = *((uint8_t *)(&Limit_N_Pos)+1);
  TxData[3] = *((uint8_t *)(&Limit_N_Pos)+2);
  TxData[4] = *((uint8_t *)(&Limit_N_Pos)+3);

  HAL_CAN_AddTxMessage(&hcan1, &TXHeader, TxData, &TxMailbox);
  // while(HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) != 3)
  // {
  //   printf("Joint_CAN_Set_Pos_N_Limit Senting...");
  //   HAL_Delay(1);
  // };
}

void Joint_CAN_Single_Command_Ctr(uint8_t Motor_ID, uint8_t cmd)
{
  uint8_t TxData = 0;

  uint32_t TxMailbox = CAN_TX_MAILBOX0;
  TXHeader.RTR = CAN_RTR_DATA;
  TXHeader.IDE = CAN_ID_STD;            
  TXHeader.StdId = Motor_ID;
  TXHeader.TransmitGlobalTime = DISABLE;
  TXHeader.DLC = Joint_CAN_DLC_1;

  TxData = cmd;

  HAL_CAN_AddTxMessage(&hcan1, &TXHeader, &TxData, &TxMailbox);
}












